#include "Communication/SubScan.h"
#include "sensor_msgs/msg/point_cloud.hpp"
#include "data_struct/geometry/PointCloud.h"
#include "lib/WrapFuncs.h"
#include "data_struct/base/DataSet.h"

namespace behavior_controller {
PointCloud<double> DataSet::scan_point_cloud;
namespace SubScan {

static rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scanSub;

void sub_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
    Funcs::FormScanToPC(*msg, DataSet::scan_point_cloud);
    DataSet::scan_point_cloud.SetTime();
}

void SubScanInfo(rclcpp::Node::SharedPtr &node_handle) {
    scanSub = node_handle->create_subscription<sensor_msgs::msg::LaserScan>(
            "/scan", rclcpp::SensorDataQoS(), sub_callback);
    std::cout << "SubScan SubScanInfo" << std::endl;
}
} // namespace SubScan

} // namespace behavior_controller